/**
* @file SensorFusionFusion.cpp in InsCubeBsp
* @author linfe
* @comment
* Create on 2022/11/1 0:01
* @version 1.0
**/


#include <memory.h>
#include <malloc.h>
#include "SensorFusion.h"
#include <app_trace.h>

#ifdef FUSION_ENGINE_MAHONY_6_AXIS
#include "Mahony/MahonyAHRS.h"
#endif
#ifdef FUSION_ENGINE_INS
#include "rgioe.h"
#include "bsp.h"
#endif

#define MAX_FUSION_FREQ 2000
#define MIN_FUSION_FREQ 1
#define MIN_DELTA_T (1.0f/MAX_FUSION_FREQ)
#define MAX_DELTA_T (1.0f/MIN_FUSION_FREQ)

SensorFusionError_t fusion_init(FusionData_t *fd, const FusionOption_t *opt) {
    memset(fd, 0, sizeof(FusionData_t));
    fd->opt = *opt;
    float deltat = opt->delta_t;
    if (deltat > MAX_DELTA_T) {
        deltat = MAX_DELTA_T;
    }
    if (deltat < MIN_DELTA_T) {
        deltat = MIN_DELTA_T;
    }
#ifdef FUSION_ENGINE_MAHONY_6_AXIS
    fd->latlng[0] =1.0f;
    fd->latlng[1] =0.123f;
    MahonyAHRSInit(1, 0, 1.0f / deltat);
#endif
#ifdef FUSION_ENGINE_INS
    fd->p_rgioe = bsp_malloc(rgioe_buffer_size);
    if(!fd->p_rgioe){
        return SensorFusionBadMalloc;
    }
//    rgioe_set_trace(fd->p_rgioe,common_trace);
    if(rgioe_init(fd->p_rgioe, NULL) != RGIOE_OK){// use default option
        return SensorFusionEngineInitFailed;
    }
#endif
    return SensorFusionOk;
}
/**
 *
 * @param timestamp
 * @param data
 * @param sensor
 * @return
 */
SensorFusionError_t fusion_update(FusionData_t *fd, const double timestamp, const void *data, Sensor_t sensor) {
    fd->cur_timestamp = timestamp;
    switch (sensor) {
        case SensorIMU: {
            fd->imu = *(ImuData_t *) data;
#ifdef FUSION_ENGINE_MAHONY_6_AXIS
            MahonyAHRSupdateIMU(fd->imu.gyro[0], fd->imu.gyro[1], fd->imu.gyro[2],
                                fd->imu.acce[0], fd->imu.acce[1], fd->imu.acce[2]
            );
#endif
#ifdef FUSION_ENGINE_INS
            RgioeImuData imu_inc = {fd->cur_timestamp,
                fd->imu.gyro[1] * fd->opt.delta_t,
                fd->imu.gyro[0] * fd->opt.delta_t,
                -fd->imu.gyro[2] * fd->opt.delta_t,
                fd->imu.acce[1] * fd->opt.delta_t,
                fd->imu.acce[0] * fd->opt.delta_t,
                -fd->imu.acce[2] * fd->opt.delta_t
            };
            /*integrate*/
            rgioe_error_t res = rgioe_timeupdate(fd->p_rgioe, timestamp, &imu_inc);
            if (res == RGIOE_IN_INITIALIZE){
                fd->status = FusionStatusAlign;
            }else if(res == RGIOE_OK){
                fd->status = FusionStatusINS;
            }else{
                fd->status = FusionStatusError;
            }
#endif
        }
            break;
        case SensorGNSS:
#ifdef FUSION_ENGINE_MAHONY_6_AXIS
#endif

#ifdef FUSION_ENGINE_INS
            rgioe_gnssupdate(fd->p_rgioe, timestamp, NULL);
#endif
            break;
        default:break;
    }
    fd->pre_timestamp = fd->cur_timestamp;
    return SensorFusionOk;
}
SensorFusionError_t fusion_refresh_result(FusionData_t *fd) {
#ifdef FUSION_ENGINE_MAHONY_6_AXIS
    MahonyAHRSGetEuler(fd->atti);
#endif
#ifdef FUSION_ENGINE_INS
    if (rgioe_get_atti(fd->p_rgioe, fd->atti, fd->atti_std) == RGIOE_OK) {
        return SensorFusionOk;
    }
#endif
    return SensorFusionGetResultFail;
}